#include "zino_imu.h"
#include "acceleration.h"
#include "gyro.h"
#include "zino.h"
#include "shanwai.h"
#include "maths.h"
#include <math.h>


#define DBG_TAG "imu"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

/**
 * @brief 开平方根求倒数
 * Fast inverse square-root
 * LINK http://en.wikipedia.org/wiki/Fast_inverse_square_root
 * @param x
 * @return float
 */
static float invSqrt(float x)
{
    // return 1.0f / sqrtf(x);
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long *)&y;             // get bits for floating value
    i = 0x5f3759df - (i >> 1);        // gives initial guess
    y = *(float *)&i;                 // convert bits back to float
    y = y * (1.5f - (halfx * y * y)); // Newton step
    return y;
}
void zino_MahonyAHRS(zino_imu_t *imu, float ax, float ay, float az, float gx, float gy, float gz, float dt)
{
    // dev->ops->acc_gyro_update(dev);
    imu->useAcc = 1;
    float ex = 0, ey = 0, ez = 0;

    gx = DEGREES_TO_RADIANS(gx);
    gy = DEGREES_TO_RADIANS(gy);
    gz = DEGREES_TO_RADIANS(gz);

    // usartPrintVariable("ax:",ax,0);
    // usartPrintVariable("\t ay:",ay,0);
    // usartPrintVariable("\t az:",az,0);
    // usartPrintVariable("\t gx:",gx,0);
    // usartPrintVariable("\t gy:",gy,0);
    // usartPrintVariable("\t gz:",gz,0);

    float recipNorm = sq(ax) + sq(ay) + sq(az);

    // usartPrintVariable("\t recipNorm:",recipNorm,1);

    if (imu->useAcc && recipNorm > 0.00001f)
    {
        // Normalise accelerometer measurement
        recipNorm = invSqrt(recipNorm);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;

        // Error is sum of cross product between estimated direction and measured direction of gravity
        ex = (ay * imu->rMat[2][2] - az * imu->rMat[2][1]);
        ey = (az * imu->rMat[2][0] - ax * imu->rMat[2][2]);
        ez = (ax * imu->rMat[2][1] - ay * imu->rMat[2][0]);
    }
    // Compute and apply integral feedback if enabled
    if (imu->ki > 0.0f)
    {
        // Stop integrating if spinning beyond the certain limit
        // if (spin_rate < SPIN_RATE_LIMIT)
        // {
        // const float dcmKiGain = imu_ki;
        imu->integralFBx += imu->ki * ex * dt; // integral error scaled by Ki
        imu->integralFBy += imu->ki * ey * dt;
        imu->integralFBz += imu->ki * ez * dt;
        // }
    }
    else
    {
        imu->integralFBx = 0.0f; // prevent integral windup
        imu->integralFBy = 0.0f;
        imu->integralFBz = 0.0f;
    }
    // if (!dev->ops->isAccCalibrated(dev))//校准时清除积分
    // {
    //     imu->integralFBx = 0.f;
    //     imu->integralFBy = 0.f;
    //     imu->integralFBz = 0.f;
    // }

    // Calculate kP gain. If we are acquiring initial attitude (not armed and within 200m sec from powerup) scale the kP to converge faster
    const float dcmKpGain = imu->kp * (millis() > 10 ? 1.0f : 10.0f);

    // Apply proportional and integral feedback
    gx += dcmKpGain * ex + imu->integralFBx;
    gy += dcmKpGain * ey + imu->integralFBy;
    gz += dcmKpGain * ez + imu->integralFBz;

    float halfT = 0.5f * dt;
    const float qa = imu->q0; // last q
    const float qb = imu->q1; // last q
    const float qc = imu->q2; // last q
    const float qd = imu->q3; // last q

    imu->q0 += (-qb * gx - qc * gy - qd * gz) * halfT;
    imu->q1 += (qa * gx + qc * gz - qd * gy) * halfT;
    imu->q2 += (qa * gy - qb * gz + qd * gx) * halfT;
    imu->q3 += (qa * gz + qb * gy - qc * gx) * halfT;
    // usartPrintVariable("q0:",imu->q0,0);
    // usartPrintVariable("q1:",imu->q1,0);
    // usartPrintVariable("q2:",imu->q2,0);
    // usartPrintVariable("q3:",imu->q3,0);
    // Normalise quaternion
    recipNorm = invSqrt(sq(imu->q0) + sq(imu->q1) + sq(imu->q2) + sq(imu->q3));
    imu->q0 = imu->q0 * recipNorm;
    imu->q1 = imu->q1 * recipNorm;
    imu->q2 = imu->q2 * recipNorm;
    imu->q3 = imu->q3 * recipNorm;

    /**
     * @brief 由四元数求得旋转矩阵R
     * LINK https://blog.csdn.net/weixin_42587961/article/details/100150826
     */
    float q1q1 = sq(imu->q1);
    float q2q2 = sq(imu->q2);
    float q3q3 = sq(imu->q3);

    float q0q1 = imu->q0 * imu->q1;
    float q0q2 = imu->q0 * imu->q2;
    float q0q3 = imu->q0 * imu->q3;
    float q1q2 = imu->q1 * imu->q2;
    float q1q3 = imu->q1 * imu->q3;
    float q2q3 = imu->q2 * imu->q3;

    imu->rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
    imu->rMat[0][1] = 2.0f * (q1q2 + -q0q3);
    imu->rMat[0][2] = 2.0f * (q1q3 - -q0q2);
    // 磁场方向
    imu->rMat[1][0] = 2.0f * (q1q2 - -q0q3);
    imu->rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
    imu->rMat[1][2] = 2.0f * (q2q3 + -q0q1);
    // 重力方向
    imu->rMat[2][0] = 2.0f * (q1q3 + -q0q2);
    imu->rMat[2][1] = 2.0f * (q2q3 - -q0q1);
    imu->rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;

    imu->attitudes.roll = lrintf(atan2f(imu->rMat[2][1], imu->rMat[2][2]) * (1800.0f / M_PIf));
    imu->attitudes.pitch = lrintf((asinf(imu->rMat[2][0])) * (1800.0f / M_PIf));
    imu->attitudes.yaw = lrintf((atan2f(imu->rMat[1][0], imu->rMat[0][0]) * (1800.0f / M_PIf)));

    // usartPrintVariable("roll:",imu->attitudes.values.roll,0);
    // usartPrintVariable("pitch:",imu->attitudes.values.pitch,0);
    // usartPrintVariable("yaw:",imu->attitudes.values.yaw,1);


    // if (imu->attitudes.yaw < 0)
    //     imu->attitudes.yaw += 3600;

    // static uint8_t swCnt= 0;
    // if(swCnt++>2)
    // {
    //     swCnt = 0;
    //     digitalOsc_Head();
    //     digitalOsc_float(ax*1000);
    //     digitalOsc_float(ay*1000);
    //     digitalOsc_float(az*1000);
    //     digitalOsc_float(gx*100.f);
    //     digitalOsc_float(gy*100.f);
    //     digitalOsc_float(gz*100.f);
    //     digitalOsc_float(imu->attitudes.roll);
    //     digitalOsc_float(imu->attitudes.pitch);
    //     digitalOsc_End();

    // }
}

zino_imu_t zino_imu = {
    .ki = 0.f,
    .kp = 0.3f,
    .q0 = 1.0f,
    .q1 = 0.f,
    .q2 = 0.f,
    .q3 = 0.f,
};
extern struct acc_device zAccDev;
extern struct gyro_device zGyroDev;
void zino_imu_task(void *p)
{
    struct acc_device *acc = &zAccDev;
    struct gyro_device *gyro = &zGyroDev;
    zino_imu_t *imu = &zino_imu;
    uint8_t try=0;
    while(1)
    {
        if(acc->init(acc) != RT_EOK)
        {
            try++;
            LOG_W("trying init acc/gyro",try);
            if(try>10)
            {
                LOG_E("init acc/gyro fail, exist.");
                return;
            }
        }
        else
        {
            LOG_I("init acc/gyro success。 IMU running...");
            break;
        }
    }
    gyro->init(gyro);
    uint32_t last_time = millis();
    while(1)
    {
        float dt = (float)(millis() - last_time) * 1e-3f;
        last_time = millis();

        acc->update(acc);
        gyro->update(gyro);

        zino_MahonyAHRS(imu, acc->value.x, acc->value.y, acc->value.z, gyro->value.x, gyro->value.y, gyro->value.z, dt);
        // ix = ix + gyro->value.x;
        // iy = iy + gyro->value.y;
        // iz = iz + gyro->value.z;


        // digitalOsc_Head();
        // digitalOsc_float(ix);
        // digitalOsc_float(iy);
        // digitalOsc_float(iz);
        // digitalOsc_float(gyro->value.x);
        // digitalOsc_float(gyro->value.y);
        // digitalOsc_float(gyro->value.z);
        // digitalOsc_float(imu->attitudes.pitch);
        // digitalOsc_float(imu->attitudes.roll);
        // digitalOsc_float(imu->attitudes.yaw);
        // digitalOsc_End();

        rt_thread_mdelay(10);
    }
}

int zino_imu_init(void)
{
    rt_thread_t tid;
    tid = rt_thread_create("imu",
                              zino_imu_task,
                              RT_NULL,
                              2048,
                              10,
                              20);
    if (tid != RT_NULL)
    {
        rt_thread_startup(tid);
        LOG_I("task startup");

    }
    else
    {
        LOG_E("task startup fail");
        return -RT_ENOMEM;
    }
    return RT_EOK;
}

// ZINO_APP_EXPORT(zino_imu_init);
